#ifndef PSO_H
#define PSO_H

#include "Vec.h"
#include "math.h"
#include <vector>

using std::vector;

//no more global best or global direction per iteration, only global best at starting and final displayed
//birds[0] store the start, birds[1] store the end.
struct particle{
		vector <point> P0s;	//record of each "particle position" over iterations
		vector <float> scores; //record of scores for each particle over iterations
		//present variables
		point P0;	//particle position (Cartesian)
		vec2 P0SC; //particle position(Spherical Coordinate)
		float score; //particle score
		float dScore; //particle score gradient
		vector <int> neighborP; //index of neighbor particle for local best
		point dLPos;	//direction from localBest to current pos of particle (velocity)
		vec2 dLPosSc;
		point dPos; //direction from previous iteration/v(t-1)
		vec2 dPosSc;
		float lBest; //local best value per iteration
		point lBestPos; //position of local best
		vec2 lBestPosSc;
		point gBestP0; //position of gBestP0 (is stored in bird[0].gBestP0)
		float gBest; //score of gBest (is stored in bird[0].gBest)
		vec2 gBestP0SC;
		//experimental
		vector <point> P0Near;
		vector <vec2> P0NearSC;
};

class particlesPSO{
public:
	
	vector<particle>birds;

	void updateNeighborPSO(int);//select neighbor from nearest particles for a particle
	void updateLBestPSO();//update local best
	void updateGBestPSO();//update global best
	void updatePosPSOsc(float threshold, float*stepSz, float *coeff, int it, int maxit);//update position
	void updtNeighborPSO(int, float);//update local best from a radial selection -->should be more accurate

};

#endif


// helper function
inline
float rand_FloatRange(float a, float b)
{
return ((b-a)*((float)rand()/RAND_MAX))+a;
}

inline
void decomposePt(point a, float &scalar, point &direction){
	scalar=0; int i=0;
	for(i=0;i<3;i++)
		scalar+=pow(a[i],2);
	scalar=sqrt(scalar);
	for(i=0;i<3;i++)
		if (scalar!=0)//avoid division by zero -->so not funny
			direction[i]=a[i]/scalar;
		else direction[i]=0;
}

inline
void decomposeVec2(vec2 a, float &scalar, vec2 &direction){
	scalar=0; int i=0;
	for(i=0;i<2;i++)
		scalar+=pow(a[i],2);
	scalar=sqrt(scalar);
	for(i=0;i<2;i++)
		if (scalar!=0)//avoid division by zero -->so not funny
			direction[i]=a[i]/scalar;
		else direction[i]=0;
}

inline
int findMinId(vector<float>input){ //find indice of the minimum value from an input array
	int i, id;
	float min=1e20;
	for(i=0;i<(int)input.size();i++)
		if(input[i]<min)
			id=i;
	return id;
}

inline
void badSortId(vector<float> input, vector<int> &idSorted, int num){ //sort in ascending order (min first) and not optimized at all ->run time will be n! until num
	int i,j,k;
	float min=1e20;
	vector <int> flag;
	for(i=0;i<(int)input.size();i++)
		flag.push_back(0);
	for(j=0;j<num;j++){
		for(i=0;i<(int)input.size();i++)
			if(flag[i]==0 && input[i]<=min){
				min=input[i];
				k=i;
			}
			flag[k]=1;
			idSorted.push_back(k);
			min=1e20;
	}
}

//phai always refer to elevation, theta for azimuth 
inline
void XYZToSphere(vec2 &s, point p){//convert spherical coord to cartesian
	float x,y,z, phai, theta;
	x=p[0]; y=p[1]; z=p[2];
	phai=acos(y/sqrt(x*x+y*y+z*z));
	theta=atan2(z, x);
	s[0]=phai; s[1]=theta;
}

inline
void sphereToXYZ(vec2 s, point &p){
	float phai, theta;
	phai=s[0]; theta=s[1];
	p[0]=cos(theta)*sin(phai);
	p[1]=cos(phai);
	p[2]=sin(theta)*sin(phai);
}

inline
float haversine(vec2 start, vec2 final, float r){// calculate geodesic distance based on haversine
	float dPhai, dTheta, arc, dist;
	dPhai=start[0]-final[0]; dTheta=start[1]-final[1];
	arc=pow(sin(dPhai/2.0),2)+cos(start[0])*cos(final[0])*pow(sin(dTheta/2.0),2);
	arc=2*asin(sqrt(arc));
	dist=r*arc;
	return dist;
}

inline
void initLocalDirs(vector<vec2>&lDirs, int numSample){//initialize vector (in 2D) for local direction of neighbor 
	int i,j;
	float range=6.2831852/numSample;
	lDirs.clear();
	vec2 a;
	for(i=0;i<numSample;i++){
		a[0]=sin(range);
		a[1]=cos(range);
		lDirs.push_back(a);
	}
}
	